//备注：拷贝代码请加上作者信息
//作者：王海涛
//邮箱：1126471088@qq.com
//版本：V1.0.0
#include "pid_q15.h"
#include <string.h>

/* 最小/最大值宏定义 */
#ifndef MAX
#define MAX(a, b) ((a) > (b) ? (a) : (b))
#endif

#ifndef MIN
#define MIN(a, b) ((a) < (b) ? (a) : (b))
#endif


/********************* 自适应调参核心函数（静态） *********************/
static void WHT_PID_Q15_Adaptive_Tune(WHT_PID_Q15_Ctrl_t *pid)
{
    if (pid == NULL || !pid->Adaptive_En) return;

    // 1. 计算Q15格式的误差绝对值、误差变化率（避免溢出）
    Q15_t err_abs = (pid->Error < 0) ? (-pid->Error) : pid->Error; // 误差绝对值
    Q15_t err_diff = (Q15_t)((int)pid->Error - pid->Error_Prev);   // 误差变化率
    Q15_t err_diff_abs = (err_diff < 0) ? (-err_diff) : err_diff;  // 变化率绝对值

    // 2. 自适应调参规则（基于误差大小和变化率，Q15格式运算）
    if (err_abs > pid->Adaptive_Err_Large)
    {
        // 场景1：误差大→优先快速跟踪，抑制超调
        pid->kp = pid->kp_max;  // 最大kp：快速拉近误差
        pid->ki = pid->ki_min;  // 最小ki：避免积分饱和
        pid->kd = pid->kd_min;  // 最小kd：减少振荡
    }
    else if (err_abs > pid->Adaptive_Err_Med)
    {
        // 场景2：误差中→平衡响应速度和稳定性（用移位实现比例缩放）
        pid->kp = (Q15_t)(((int)pid->kp_max + pid->kp_min) >> 1);        // kp=平均值（/2）
        pid->ki = (Q15_t)MIN((int)pid->ki_min << 1, pid->ki_max);        // ki=2*最小值（不超过max）
        pid->kd = (Q15_t)(((int)pid->kd_min + (pid->kd_max >> 1)) >> 1); // kd=（min + max/2）/2
    }
    else
    {
        // 场景3：误差小→优先稳定和消静差
        if (err_diff_abs < pid->Adaptive_Diff_Small)
        {
            // 变化慢→增大ki消静差，减小kp防振荡
            pid->kp = (Q15_t)MIN((int)pid->kp_min << 1, pid->kp_max); // kp=2*最小值
            pid->ki = pid->ki_max;                                    // 最大ki：快速消静差
            pid->kd = pid->kd_min;                                    // 最小kd：不影响稳定
        }
        else
        {
            // 变化快→增大kd抑制振荡，减小ki防饱和
            pid->kp = (Q15_t)(((int)pid->kp_min + (pid->kp_max >> 1)) >> 1); // kp=中等
            pid->ki = (Q15_t)MAX((int)pid->ki_max >> 1, pid->ki_min);        // ki=max/2（不低于min）
            pid->kd = pid->kd_max;                                           // 最大kd：抑制振荡
        }
    }
}

/**
 * @brief  PID初始化函数
 * @details 初始化PID参数、限幅、滤波、自适应参数等默认配置
 */
static void WHT_PID_Q15_Init(WHT_PID_Q15_Ctrl_t *pid, Q15_t kp, Q15_t ki, Q15_t kd, Q15_t output_min, Q15_t output_max)
{
    if (pid == NULL) return;
    
    // 初始化PID系数
    pid->kp = kp;
    pid->ki = ki;
    pid->kd = kd;
    
    // 初始化设定值和反馈值
    pid->Setpoint = 0;
    pid->Feedback = 0;
    pid->Error = 0;
    pid->Error_Prev = 0;
    pid->Error_Sum = 0;
    
    // 初始化输出限幅
    pid->Output_Min = output_min;
    pid->Output_Max = output_max;
    
    // 初始化积分限幅（默认禁用）
    pid->Integral_Limit_En = false;
    pid->Integral_Min = WHT_PID_Q15.WHT_Float_To_Q15(-1.0f);
    pid->Integral_Max = WHT_PID_Q15.WHT_Float_To_Q15(1.0f);
    
    // 初始化微分滤波（默认禁用，系数0.9）
    pid->Derivative_Filter_En = false;
    pid->Derivative_Alpha = WHT_PID_Q15.WHT_Float_To_Q15(0.9f);
    pid->Derivative_Filtered = 0;

    // 初始化自适应参数（默认禁用，范围设为安全值）
    pid->Adaptive_En = false;                          // 默认禁用自适应
    pid->kp_min = WHT_PID_Q15.WHT_Float_To_Q15(0.1f);  // 默认kp最小=0.1
    pid->kp_max = WHT_PID_Q15.WHT_Float_To_Q15(1.0f);  // 默认kp最大=1.0
    pid->ki_min = WHT_PID_Q15.WHT_Float_To_Q15(0.01f); // 默认ki最小=0.01
    pid->ki_max = WHT_PID_Q15.WHT_Float_To_Q15(0.5f);  // 默认ki最大=0.5
    pid->kd_min = WHT_PID_Q15.WHT_Float_To_Q15(0.05f); // 默认kd最小=0.05
    pid->kd_max = WHT_PID_Q15.WHT_Float_To_Q15(0.8f);  // 默认kd最大=0.8
    // 默认误差阈值（Q15格式，对应十进制：0.125、0.0625、0.03125）
    pid->Adaptive_Err_Large = 0x1000;
    pid->Adaptive_Err_Med = 0x0800;
    pid->Adaptive_Diff_Small = 0x0400;
}

/**
 * @brief  配置积分限幅
 */
static void WHT_PID_Q15_Config_Integral_Limit(WHT_PID_Q15_Ctrl_t *pid, bool enable, Q15_t integral_min, Q15_t integral_max)
{
    if (pid == NULL) return;
    
    pid->Integral_Limit_En = enable;
    pid->Integral_Min = integral_min;
    pid->Integral_Max = integral_max;
    
    // 立即对当前积分值进行限幅
    if (enable)
    {
        if (pid->Error_Sum > integral_max)      pid->Error_Sum = integral_max;
        else if (pid->Error_Sum < integral_min) pid->Error_Sum = integral_min;
    }
}

/**
 * @brief  配置微分滤波
 */
static void WHT_PID_Q15_Config_Derivative_Filter(WHT_PID_Q15_Ctrl_t *pid, bool enable, Q15_t alpha)
{
    if (pid == NULL) return;
    
    pid->Derivative_Filter_En = enable;
    // 确保滤波系数在合理范围（Q15格式的0~1）
    Q15_t alpha_max = WHT_PID_Q15.WHT_Float_To_Q15(1.0f);
    if (alpha < 0)               pid->Derivative_Alpha = 0;
    else if (alpha > alpha_max)  pid->Derivative_Alpha = alpha_max;
    else                         pid->Derivative_Alpha = alpha;
}

/**
 * @brief  设置PID设定值
 */
static void WHT_PID_Q15_Set_Setpoint(WHT_PID_Q15_Ctrl_t *pid, Q15_t setpoint)
{
    if (pid == NULL) return;
    pid->Setpoint = setpoint;
}

/**
 * @brief  PID计算核心函数
 * @details 采用位置式PID算法，包含积分限幅、微分滤波、输出限幅、自适应调参
 */
static Q15_t WHT_PID_Q15_Compute(WHT_PID_Q15_Ctrl_t *pid, Q15_t feedback)
{
    if (pid == NULL) return 0;
    
    int temp;  // 临时变量（用于避免溢出）
    Q15_t p_out, i_out, d_out, total_out;
    
    // 1. 更新反馈值和当前误差
    pid->Feedback = feedback;
    pid->Error = (Q15_t)((int)pid->Setpoint - feedback);  // 误差 = 设定值 - 反馈值

    /********************* 自适应调参（启用时执行） *********************/
    WHT_PID_Q15_Adaptive_Tune(pid);

    // 2. 比例项计算（Q15*Q15=Q30→右移15位转Q15）
    temp = (int)pid->kp * pid->Error;
    p_out = (Q15_t)(temp >> 15);
    
    // 3. 积分项计算
    temp = (int)pid->Error_Sum + pid->Error;  // 积分累计（Q15+Q15=Q16，无溢出）
    pid->Error_Sum = (Q15_t)temp;             // 转回Q15
    // 积分限幅
    if (pid->Integral_Limit_En)
    {
        if (pid->Error_Sum > pid->Integral_Max)      pid->Error_Sum = pid->Integral_Max;
        else if (pid->Error_Sum < pid->Integral_Min) pid->Error_Sum = pid->Integral_Min;
    }
    // 积分项输出
    temp = (int)pid->ki * pid->Error_Sum;
    i_out = (Q15_t)(temp >> 15);
    
    // 4. 微分项计算
    temp = (int)pid->Error - pid->Error_Prev;  // 误差变化率
    Q15_t derivative = (Q15_t)temp;
    // 微分滤波
    if (pid->Derivative_Filter_En)
    {
        // 一阶低通：filtered = alpha*current + (1-alpha)*previous
        temp = (int)pid->Derivative_Alpha * derivative;
        Q15_t alpha_part = (Q15_t)(temp >> 15);
        Q15_t alpha_max = WHT_PID_Q15.WHT_Float_To_Q15(1.0f);
        temp = (int)(alpha_max - pid->Derivative_Alpha) * pid->Derivative_Filtered;
        Q15_t beta_part = (Q15_t)(temp >> 15);
        pid->Derivative_Filtered = (Q15_t)((int)alpha_part + beta_part);
        derivative = pid->Derivative_Filtered;
    }
    // 微分项输出
    temp = (int)pid->kd * derivative;
    d_out = (Q15_t)(temp >> 15);
    
    // 5. 总输出计算和限幅
    temp = (int)p_out + i_out + d_out;
    if (temp > pid->Output_Max)      total_out = pid->Output_Max;
    else if (temp < pid->Output_Min) total_out = pid->Output_Min;
    else                             total_out = (Q15_t)temp;
    
    // 6. 更新上一次误差
    pid->Error_Prev = pid->Error;
    
    return total_out;
}

/**
 * @brief  重置PID状态
 */
static void WHT_PID_Q15_Reset(WHT_PID_Q15_Ctrl_t *pid)
{
    if (pid == NULL) return;
    
    pid->Error = 0;
    pid->Error_Prev = 0;
    pid->Error_Sum = 0;
    pid->Derivative_Filtered = 0;
    pid->Feedback = 0;
}

/**
 * @brief  十进制转Q15格式
 */
static Q15_t WHT_PID_Q15_Float_To_Q15(float val)
{
    // 范围钳位：避免超出Q15的[-1.0, 1.0)
    if (val >= 1.0f) return 0x7FFF;
    if (val <= -1.0f) return 0x8000;
    return (Q15_t)(val * 32768.0f);
}

/**
 * @brief  Q15格式转十进制
 */
static float WHT_PID_Q15_Q15_To_Float(Q15_t q)
{
    return (float)q / 32768.0f;
}

/**
 * @brief  自适应配置函数
 */
static void WHT_PID_Q15_Config_Adaptive(WHT_PID_Q15_Ctrl_t *pid, bool enable, Q15_t kp_min, Q15_t kp_max, Q15_t ki_min, Q15_t ki_max, Q15_t kd_min, Q15_t kd_max, Q15_t err_large, Q15_t err_med, Q15_t diff_small)
{
    if (pid == NULL) return;

    // 1. 配置自适应使能和系数范围（确保范围合法）
    pid->Adaptive_En = enable;
    pid->kp_min = kp_min < 0 ? 0 : kp_min;                      // kp最小≥0
    pid->kp_max = MAX(kp_min, kp_max);                          // kp最大≥最小
    pid->ki_min = ki_min < 0 ? 0 : ki_min;                      // ki最小≥0
    pid->ki_max = MAX(ki_min, ki_max);                          // ki最大≥最小
    pid->kd_min = kd_min < 0 ? 0 : kd_min;                      // kd最小≥0
    pid->kd_max = MAX(kd_min, kd_max);                          // kd最大≥最小

    // 2. 配置误差阈值（确保阈值合法：大>中>0，变化率阈值>0）
    pid->Adaptive_Err_Large = MAX(err_med, err_large);          // 大阈值≥中阈值
    pid->Adaptive_Err_Med = err_med < 0 ? 0 : err_med;          // 中阈值≥0
    pid->Adaptive_Diff_Small = diff_small < 0 ? 0 : diff_small; // 变化率阈值≥0
}

/* PID函数表初始化 */
const WHT_PID_Q15_t WHT_PID_Q15 =
{
    .WHT_Init                     = WHT_PID_Q15_Init,
    .WHT_Reset                    = WHT_PID_Q15_Reset,
    .WHT_Config_Integral_Limit    = WHT_PID_Q15_Config_Integral_Limit,
    .WHT_Config_Derivative_Filter = WHT_PID_Q15_Config_Derivative_Filter,
    .WHT_Set_Setpoint             = WHT_PID_Q15_Set_Setpoint,
    .WHT_Compute                  = WHT_PID_Q15_Compute,
    .WHT_Float_To_Q15             = WHT_PID_Q15_Float_To_Q15,
    .WHT_Q15_To_Float             = WHT_PID_Q15_Q15_To_Float,
    .WHT_Config_Adaptive          = WHT_PID_Q15_Config_Adaptive,
};
